#include "exp_node.h"
#include "topology.h"
#include "LMI.h"
#include "obs.h"

const int     N_drone = 4;        // 无人机数目（包含领导者）
const int     N_UGV = 1;          // 无人车数目
const double  H_flight_plain = 2; // 无人机飞行高度（固定）
const int     N_obs = 4;          // 障碍物数目

// 领导者部分参数 状态量为px,vx,py,vy
// 轨迹参数
double r_leader = 10.0;  // 运动范围
double omega_leader = 0.3;  // 运动速度
double alpha_p = -omega_leader * omega_leader;
double alpha_v = -0.05;     // 取零会使其振荡运动
// 状态矩阵
Eigen::Matrix4d S = (Eigen::Matrix4d() << 
        0, 1, 0, 0,
        alpha_p, alpha_v, 0, 0,
        0, 0, 0, 1,
        0, 0, alpha_p, alpha_v).finished();

Eigen::MatrixXd F = (Eigen::MatrixXd(2,4) << 1, 0, 0, 0,
                                             0, 0, 1, 0).finished();

// 跟随者部分参数
// 无人机UAV
double alpha_p_uav = -5;
double alpha_v_uav = -3;
// 状态矩阵
Eigen::Matrix4d A_uav = (Eigen::Matrix4d() <<
        0, 1, 0, 0,
        alpha_p_uav, alpha_v_uav, 0, 0,
        0, 0, 0, 1,
        0, 0, alpha_p_uav, alpha_v_uav).finished();
Eigen::MatrixXd B_uav = (Eigen::MatrixXd(4,2) << 0, 0,
                                                 1, 0,
                                                 0, 0,
                                                 0, 1).finished();
Eigen::MatrixXd C_uav = (Eigen::MatrixXd(2,4) << 1, 0, 0, 0,
                                                 0, 0, 1, 0).finished();
Eigen::Matrix4d X_uav = Eigen::Matrix4d::Identity();   
Eigen::MatrixXd U_uav = (Eigen::MatrixXd(2,4) << alpha_p-alpha_p_uav, alpha_v-alpha_v_uav, 0, 0,
                                                 0, 0, alpha_p-alpha_p_uav, alpha_v-alpha_v_uav).finished();

Eigen::MatrixXd K1_uav = (Eigen::MatrixXd(2,4) << -1.3, -1.4, 0, 0,
                                                  0, 0, -1.3, -1.4).finished();  // 这里后续需要调参，预设部分就是自动生成了                
Eigen::MatrixXd K2_uav = U_uav - K1_uav * X_uav;

// 无人车UGV
// 状态矩阵
Eigen::Matrix2d A_ugv = Eigen::Matrix2d::Zero();
Eigen::Matrix2d B_ugv = Eigen::Matrix2d::Identity();
Eigen::Matrix2d C_ugv = Eigen::Matrix2d::Identity();
Eigen::MatrixXd X_ugv = (Eigen::MatrixXd(2,4) << 1, 0, 0, 0,
                                                 0, 0, 1, 0).finished();
Eigen::MatrixXd U_ugv = (Eigen::MatrixXd(2,4) << 0, 1, 0, 0,
                                                 0, 0, 0, 1).finished();
Eigen::MatrixXd K1_ugv = (Eigen::MatrixXd(2,2) << -0.5, 0,
                                                  0, -0.5).finished();  // 这里后续需要调参，预设部分就是自动生成了                
Eigen::MatrixXd K2_ugv = U_ugv - K1_ugv * X_ugv;

// 初始值
// 初始观测值 每一列是一个智能体，从上至下为px vx py vy
Eigen::Matrix4d init_obrv = (Eigen::Matrix4d() << -0.1, 0.2, 0.3, -0.4,
                                                  -0.5, 0.6, 0.7, 0.8,
                                                   0.7, 0.8, 0.9, 0.8,
                                                  -0.7, 0.6, 0.5, 0.4).finished();

Eigen::Matrix4d obrv_esti = init_obrv;

// 障碍物位置
Eigen::MatrixXd obs_pos = (Eigen::MatrixXd(2, 5) << 0, 0, 0, 0, 0,
                                                    0, 0, 0, 0, 0).finished();

h_t get_funh(int k, double t)
{
    h_t h_out;
    double r = 1.5;           // 编队半径
    double omega = 0.1;     // 编队角速度
    double pi = M_PI;       // 圆周率
    k = k - 1;
    h_out.px = r * cos(omega * t + 2 * pi * k / (N_drone + N_UGV - 1));
    h_out.vx = -omega * r * sin(omega * t + 2 * pi * k / (N_drone + N_UGV - 1));
    h_out.ax = -omega * omega * r * cos(omega * t + 2 * pi * k / (N_drone + N_UGV - 1));
    h_out.py = r * sin(omega * t + 2 * pi * k / (N_drone + N_UGV - 1));
    h_out.vy = omega * r * cos(omega * t + 2 * pi * k / (N_drone + N_UGV - 1));
    h_out.ax = -omega * omega * r * sin(omega * t + 2 * pi * k / (N_drone + N_UGV - 1));
    return h_out;
}

class Leader {
private:
    ros::ServiceClient arming_client_;
    // 订阅话题和发布话题
    ros::Subscriber state_sub_;
    ros::Subscriber pose_sub_;
    ros::Subscriber velocity_sub_;
    ros::Publisher control_pub_;
    ros::ServiceClient set_mode_client_;
    // double expect_dest[2] = {10, 5}; //x, y

public:
    // 无人机名
    std::string name_;
    // ROS节点句柄
    ros::NodeHandle nh_;
    mavros_msgs::SetMode offb_set_mode;
    mavros_msgs::State current_state_;              // 当前飞控marvos状态数据
    geometry_msgs::PoseStamped current_pose_;       // 当前位置
    geometry_msgs::TwistStamped current_velocity_;  // 当前速度

    // 构造函数 ############################################################################
    Leader(const std::string& name) : name_(name) {
        // 解锁/模式切换客户端
        arming_client_ = nh_.serviceClient<mavros_msgs::CommandBool>("/uav" + name_ + "/mavros/cmd/arming");    // 无人机解锁
        set_mode_client_ = nh_.serviceClient<mavros_msgs::SetMode>("/uav" + name_ + "/mavros/set_mode");        // 无人机飞行模式
        // 初始化ROS订阅者和发布者 
        // 订阅无人机的飞控参数、位置与速度，发布位置输出量
        state_sub_ = nh_.subscribe<mavros_msgs::State>("/uav" + name_ + "/mavros/state", 1000, &Leader::stateCallback, this);
        pose_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("/uav" + name_ + "/mavros/local_position/pose", 1000, &Leader::poseCallback, this);
        velocity_sub_ = nh_.subscribe<geometry_msgs::TwistStamped>("/uav" + name_ + "/mavros/local_position/velocity_local", 1000, &Leader::velocityCallback, this);
        control_pub_ = nh_.advertise<mavros_msgs::PositionTarget>("/uav" + name_ + "/mavros/setpoint_raw/local", 1000);
        
        // 请求无人机的状态
    }

    // 析构函数 清理内存
    ~Leader(void) {
        // 确保在退出时停止无人机
    }

    // 回调函数 ############################################################################
    // 飞控marvos状态数据回调函数
    void stateCallback(const mavros_msgs::State::ConstPtr& msg) {
        current_state_ = *msg;
    }

    // 位置回调函数
    void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_ = *msg;
    }

    // 速度回调函数
    void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_ = *msg;
    }

    // 无人机运动指令 ############################################################################
    // 定点保持
    void stay(void)
    {
        static int stay_mark = 0;
        static mavros_msgs::PositionTarget stay_pt;
        stay_pt.coordinate_frame = 1;       // 指定参考坐标系
        stay_pt.type_mask = 0b111111111000; // 掩码（从右往左，px py pz vx vy vz ax ay az F yaw yaw_rate）1为忽视，0为保留
        if (stay_mark == 0)
        {
            stay_pt.position.x = current_pose_.pose.position.x;
            stay_pt.position.y = current_pose_.pose.position.y;
            stay_pt.position.z = current_pose_.pose.position.z;
            stay_mark = 1;
        }
        control_pub_.publish(stay_pt);
    }

    // 起飞
    void takeoff(void)
    {
        static int takeoff_mark = 0;
        static mavros_msgs::PositionTarget takeoff_pt;
        takeoff_pt.coordinate_frame = 1;
        takeoff_pt.type_mask = 0b111111111000;
        if (takeoff_mark == 0)
        {
            takeoff_pt.position.x = current_pose_.pose.position.x;
            takeoff_pt.position.y = current_pose_.pose.position.y;
            takeoff_pt.position.z = H_flight_plain;     // 上升到指定高度
            takeoff_mark = 1;
        }
        control_pub_.publish(takeoff_pt);
    }

    // 停止，用于实验完成后停止编队转圈，速度归零
    void stop(void)
    {
        static mavros_msgs::PositionTarget stop_pt;
        stop_pt.coordinate_frame = 1;
        stop_pt.type_mask = 0b111111000111;
        stop_pt.velocity.x = 0;
        stop_pt.velocity.y = 0;
        stop_pt.velocity.z = 0;
        control_pub_.publish(stop_pt);
    }

    // 着陆
    void land(void)
    {
        static int land_mark = 0;
        static mavros_msgs::PositionTarget land_pt;
        land_pt.coordinate_frame = 1;
        land_pt.type_mask = 0b111111111000;
        if (land_mark == 0)
        {
            land_pt.position.x = current_pose_.pose.position.x;
            land_pt.position.y = current_pose_.pose.position.y;
            land_pt.position.z = 0;
            land_mark = 1;
        }
        control_pub_.publish(land_pt);
    }

    // 真正的算法部分，计算步进，在主函数里算法的周期为0.01s 即algorithm_rate=100Hz
    void step()
    {
        mavros_msgs::PositionTarget step_pt;
        step_pt.coordinate_frame = 1;
        step_pt.type_mask = 0b110000111011;     // 将无人机作为二阶积分器模型，领导者没有输入量，只需加速度和力 同时保持高度
        step_pt.acceleration_or_force.x = alpha_p * (current_pose_.pose.position.x) + alpha_v * current_velocity_.twist.linear.x;
        step_pt.acceleration_or_force.y = alpha_p * (current_pose_.pose.position.y) + alpha_v * current_velocity_.twist.linear.y;
        step_pt.acceleration_or_force.z = 0;
        step_pt.position.z = H_flight_plain;
        control_pub_.publish(step_pt);
    }

    // 打开offboard模式
    void setoffboard()
    {
        offb_set_mode.request.custom_mode = "OFFBOARD"; // 外部控制，非手操
        if( current_state_.mode != "OFFBOARD")
            //客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了offboard模式
            if(set_mode_client_.call(offb_set_mode) && offb_set_mode.response.mode_sent)
                //打开Offboard模式后在终端打印信息
                ROS_INFO("[%s]: Offboard enabled", name_.c_str());
    }

    // 检查offboard模式
    bool checkoffboard()
    {
        if( current_state_.mode == "OFFBOARD")
            return true;
        else
            return false;
    }
};

// 无人机跟随者 ############################################################################
class Follower_UAV {
private:
    ros::ServiceClient arming_client_;
    // 订阅话题和发布话题
    ros::Subscriber state_sub_;
    ros::Subscriber pose_sub_[5];       // 0 领导者 1~3UAV 4UGV
    ros::Subscriber velocity_sub_[5];   // 
    ros::Publisher control_pub_;
    ros::ServiceClient set_mode_client_;
    // double alpha[2] = {-5, -5};      // 阻尼系数

public:
    // 无人机名
    std::string name_;
    int num_;
    // ROS节点句柄
    ros::NodeHandle nh_;
    mavros_msgs::SetMode offb_set_mode;
    mavros_msgs::State current_state_;
    geometry_msgs::PoseStamped current_pose_[N_drone+N_UGV];
    geometry_msgs::TwistStamped current_velocity_[N_drone+N_UGV];

    // 构造函数 ############################################################################
    Follower_UAV(const std::string& name) : name_(name) {
        // 解锁/模式切换客户端
        num_ = std::stoi(name_) - 1;
        arming_client_ = nh_.serviceClient<mavros_msgs::CommandBool>("/uav" + name_ + "/mavros/cmd/arming");
        set_mode_client_ = nh_.serviceClient<mavros_msgs::SetMode>("/uav" + name_ + "/mavros/set_mode");
        // 初始化ROS订阅者和发布者
        state_sub_ = nh_.subscribe<mavros_msgs::State>("/uav" + name_ + "/mavros/state", 1000, &Follower_UAV::stateCallback, this);
        
        // 0对应领导者
        // 1~3对应跟随者无人机
        pose_sub_[0] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav1/mavros/local_position/pose", 1000, &Follower_UAV::poseCallback1, this);
        velocity_sub_[0] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav1/mavros/local_position/velocity_local", 1000, &Follower_UAV::velocityCallback1, this);
        pose_sub_[1] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav2/mavros/local_position/pose", 1000, &Follower_UAV::poseCallback2, this);
        velocity_sub_[1] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav2/mavros/local_position/velocity_local", 1000, &Follower_UAV::velocityCallback2, this);
        pose_sub_[2] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav3/mavros/local_position/pose", 1000, &Follower_UAV::poseCallback3, this);
        velocity_sub_[2] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav3/mavros/local_position/velocity_local", 1000, &Follower_UAV::velocityCallback3, this);
        pose_sub_[3] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav4/mavros/local_position/pose", 1000, &Follower_UAV::poseCallback4, this);
        velocity_sub_[3] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav4/mavros/local_position/velocity_local", 1000, &Follower_UAV::velocityCallback4, this);
        pose_sub_[4] = nh_.subscribe<geometry_msgs::PoseStamped>("/ugv1/pose", 1000, &Follower_UAV::poseCallback5, this);
        velocity_sub_[4] = nh_.subscribe<geometry_msgs::TwistStamped>("/ugv1/twist", 1000, &Follower_UAV::velocityCallback5, this);

        control_pub_ = nh_.advertise<mavros_msgs::PositionTarget>("/uav" + name_ + "/mavros/setpoint_raw/local", 1000);
        // 请求无人机的状态
    }

    // 析构函数
    ~Follower_UAV() {
        // 确保在退出时停止无人机
    }

    // 回调函数 ############################################################################
    // 无人机飞控参数回调函数
    void stateCallback(const mavros_msgs::State::ConstPtr& msg) {
        current_state_ = *msg;
    }

    // 位置与速度回调
    void poseCallback1(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[0] = *msg;
    }
    void velocityCallback1(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[0] = *msg;
    }

    void poseCallback2(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[1] = *msg;
    }
    void velocityCallback2(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[1] = *msg;
    }

    void poseCallback3(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[2] = *msg;
    }
    void velocityCallback3(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[2] = *msg;
    }

    void poseCallback4(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[3] = *msg;
    }
    void velocityCallback4(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[3] = *msg;
    }

    void poseCallback5(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[4] = *msg;
    }
    void velocityCallback5(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[4] = *msg;
    }

    // 动作函数 ############################################################################
    // 保持
    void stay()
    {
        static int stay_mark[4] = {0, 0, 0, 0};
        static mavros_msgs::PositionTarget stay_pt[4];
        stay_pt[num_].coordinate_frame = 1;
        stay_pt[num_].type_mask = 0b111111111000;
        if (stay_mark[num_] == 0)
        {
            // ROS_INFO("[%d]:stay_mark has been set to 1", num_);
            stay_pt[num_].position.x = current_pose_[num_].pose.position.x;
            stay_pt[num_].position.y = current_pose_[num_].pose.position.y;
            stay_pt[num_].position.z = current_pose_[num_].pose.position.z;
            stay_mark[num_] = 1;
        }
        control_pub_.publish(stay_pt[num_]);

        // ROS_INFO("[%d]:Current Target Pos: ([%f],[%f])", num_, current_pose_[num_].pose.position.x, current_pose_[num_].pose.position.y);
        // ROS_INFO("[%d]:Current Target Pos: ([%f],[%f])", num_, stay_pt[num_].position.x, stay_pt[num_].position.y);
    }

    // 起飞
    void takeoff()
    {
        static int takeoff_mark[4] = {0, 0, 0, 0};
        static mavros_msgs::PositionTarget takeoff_pt[4];
        takeoff_pt[num_].coordinate_frame = 1;
        takeoff_pt[num_].type_mask = 0b111111111000;
        if (takeoff_mark[num_] == 0)
        {
            // ROS_INFO("[%d]:takeoff_mark has been set to 1", num_);
            takeoff_pt[num_].position.x = current_pose_[num_].pose.position.x;
            takeoff_pt[num_].position.y = current_pose_[num_].pose.position.y;
            takeoff_pt[num_].position.z = H_flight_plain;
            takeoff_mark[num_] = 1;
        }
        control_pub_.publish(takeoff_pt[num_]);
        // ROS_INFO("[%d]:Current Target Pos: ([%f],[%f])", num_, takeoff_pt[num_].position.x, takeoff_pt[num_].position.y);
    }

    // 停止
    void stop()
    {
        static mavros_msgs::PositionTarget stop_pt;
        stop_pt.coordinate_frame = 1;
        stop_pt.type_mask = 0b111111000111;
        stop_pt.velocity.x = 0;
        stop_pt.velocity.y = 0;
        stop_pt.velocity.z = 0;
        control_pub_.publish(stop_pt);
    }

    // 着陆
    void land()
    {
        static int land_mark[4] = {0, 0, 0, 0};
        static mavros_msgs::PositionTarget land_pt[4];
        land_pt[num_].coordinate_frame = 1;
        land_pt[num_].type_mask = 0b111111111000;
        if (land_mark[num_] == 0)
        {
            // ROS_INFO("[%d]:land_mark has been set to 1", num_);
            land_pt[num_].position.x = current_pose_[num_].pose.position.x;
            land_pt[num_].position.y = current_pose_[num_].pose.position.y;
            land_pt[num_].position.z = 0;
            land_mark[num_] = 1;
        }
        control_pub_.publish(land_pt[num_]);
        // ROS_INFO("[%d]:Current Target Pos: ([%f],[%f])", num_, land_pt[num_].position.x, land_pt[num_].position.y);
    }

    // 步进
    void step(int sigma, double t)
    {
        // static double last_t;
        double ux = 0, uy = 0;
        h_t h_current;
        Eigen::VectorXd avoid_feedback_value = avoid_feedback(current_pose_, obs_pos, num_);
        Eigen::VectorXd diff_feedback = (avoid_feedback_value - feedback_temp.col(num_-1)) / 0.01;
        feedback_temp.col(num_-1) = avoid_feedback_value;

        // 编队向量 0~2为跟随者无人机 3为无人车 hpx hvx hax hpy hvy hay  
        h_current = get_funh(num_, t);

        // 加入防碰撞反馈
        h_current.px += avoid_feedback_value(0);
        h_current.py += avoid_feedback_value(1);
        h_current.vx += diff_feedback(0);
        h_current.vy += diff_feedback(1); 

        // 编队补偿 
        Eigen::MatrixXd tau = (Eigen::MatrixXd(2,1) << h_current.ax - alpha_p_uav * h_current.px - alpha_v_uav * h_current.vx, 
                                                       h_current.ay - alpha_p_uav * h_current.py - alpha_v_uav * h_current.vy).finished(); 

        // tau << h_current.vx, h_current.vy;

        // 状态量
        Eigen::MatrixXd state = (Eigen::MatrixXd(4,1) << 
                current_pose_[num_].pose.position.x, current_velocity_[num_].twist.linear.x, 
                current_pose_[num_].pose.position.y, current_velocity_[num_].twist.linear.y).finished();
        // 拓展编队向量
        Eigen::MatrixXd phi= (Eigen::MatrixXd(4,1) << 
                h_current.px, h_current.vx, 
                h_current.py, h_current.vy).finished();
        // 输入量
        Eigen::MatrixXd input = K1_uav * (state - phi) + K2_uav * obrv_esti.col(num_-1) + tau;
        
        // Ax 除了控制输入外的部分
        ux += input(0,0) + alpha_p_uav * current_pose_[num_].pose.position.x + alpha_v_uav * current_velocity_[num_].twist.linear.x;
        uy += input(1,0) + alpha_p_uav * current_pose_[num_].pose.position.y + alpha_v_uav * current_velocity_[num_].twist.linear.y;

        // 观测器
        double error_vx = 0;
        double error_vy = 0;
        // 跟随者之间
        for(int i = 0; i < N_drone+N_UGV-1; i++)
        {
            if(Wmat[sigma](num_, i+1))
            {
                error_vx += obrv_esti(0, num_-1) - obrv_esti(0, i);   // PX
                error_vy += obrv_esti(2, num_-1) - obrv_esti(2, i);   // PY
            }
        }
        // 跟随者与领导者
        if(Wmat[sigma](num_, 0))
        {
            error_vx += obrv_esti(0, num_-1) - current_pose_[0].pose.position.x;
            error_vy += obrv_esti(2, num_-1) - current_pose_[0].pose.position.y;
        }

        Eigen::MatrixXd error = (Eigen::MatrixXd(2,1) << error_vx, error_vy).finished();
        Eigen::MatrixXd diff_v = S*obrv_esti.col(num_-1) - Ko*error;

        obrv_esti.col(num_-1) += diff_v * 0.01; // 积分

        static mavros_msgs::PositionTarget step_pt;
        step_pt.coordinate_frame = 1;
        step_pt.type_mask = 0b110000111011;
        step_pt.acceleration_or_force.x = ux;
        step_pt.acceleration_or_force.y = uy;
        step_pt.acceleration_or_force.z = 0;
        step_pt.position.z = H_flight_plain;
        control_pub_.publish(step_pt);
    }

    void setoffboard_fol_uav()
    {
        offb_set_mode.request.custom_mode = "OFFBOARD";
        if( current_state_.mode != "OFFBOARD")
        {
            //客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了offboard模式
            if(set_mode_client_.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                //打开Offboard模式后在终端打印信息
                ROS_INFO("[%s]: Offboard enabled", name_.c_str());
            }
        }
    }

    bool checkoffboard_fol_uav()
    {
        if( current_state_.mode == "OFFBOARD")
            return true;
        else
            return false;
    }
};

class Follower_UGV {
    private:
        // 订阅话题和发布话题
        ros::Subscriber pose_sub_[5];
        ros::Subscriber velocity_sub_[5];
        ros::Publisher control_pub_;

    public:
        // 无人车
        std::string name_;
        int num_;
        // ROS节点句柄
        ros::NodeHandle nh_;
        geometry_msgs::PoseStamped current_pose_[N_drone+N_UGV];
        geometry_msgs::TwistStamped current_velocity_[N_drone+N_UGV];

        // 构造函数 ############################################################################
        Follower_UGV(const std::string& name) : name_(name) {
            // 初始化ROS订阅者和发布者
            num_ = std::stoi(name_) + N_drone - 1;
            // 0对应领导者 uav1
            // 1~3对应跟随者无人机 uav2~uav4
            // 4 对应无人车 ugv1
            pose_sub_[0] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav1/mavros/local_position/pose", 1000, &Follower_UGV::poseCallback1, this);
            velocity_sub_[0] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav1/mavros/local_position/velocity_local", 1000, &Follower_UGV::velocityCallback1, this);
            pose_sub_[1] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav2/mavros/local_position/pose", 1000, &Follower_UGV::poseCallback2, this);
            velocity_sub_[1] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav2/mavros/local_position/velocity_local", 1000, &Follower_UGV::velocityCallback2, this);
            pose_sub_[2] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav3/mavros/local_position/pose", 1000, &Follower_UGV::poseCallback3, this);
            velocity_sub_[2] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav3/mavros/local_position/velocity_local", 1000, &Follower_UGV::velocityCallback3, this);
            pose_sub_[3] = nh_.subscribe<geometry_msgs::PoseStamped>("/uav4/mavros/local_position/pose", 1000, &Follower_UGV::poseCallback4, this);
            velocity_sub_[3] = nh_.subscribe<geometry_msgs::TwistStamped>("/uav4/mavros/local_position/velocity_local", 1000, &Follower_UGV::velocityCallback4, this);
            pose_sub_[4] = nh_.subscribe<geometry_msgs::PoseStamped>("/ugv1/pose", 1000, &Follower_UGV::poseCallback5, this);
            velocity_sub_[4] = nh_.subscribe<geometry_msgs::TwistStamped>("/ugv1/twist", 1000, &Follower_UGV::velocityCallback5, this);
        
            control_pub_ = nh_.advertise<geometry_msgs::Twist>("/ugv" + name_ + "/cmd_vel", 1000);
        }

        // 析构函数
    ~Follower_UGV() {
        // 确保在退出时停止无人机
    }

    // 回调函数 ############################################################################
    // 位置与速度回调1~5
    void poseCallback1(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[0] = *msg;
    }
    void velocityCallback1(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[0] = *msg;
    }

    void poseCallback2(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[1] = *msg;
    }
    void velocityCallback2(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[1] = *msg;
    }

    void poseCallback3(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[2] = *msg;
    }
    void velocityCallback3(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[2] = *msg;
    }

    void poseCallback4(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[3] = *msg;
    }
    void velocityCallback4(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[3] = *msg;
    }

    void poseCallback5(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        current_pose_[4] = *msg;
    }
    void velocityCallback5(const geometry_msgs::TwistStamped::ConstPtr& msg) {
        current_velocity_[4] = *msg;
    }

    // 动作函数 ############################################################################
    // 保持
    void stay()
    {
        static int stay_mark[4] = {0, 0, 0, 0};
        static geometry_msgs::Twist stay_pt[4];
        if (stay_mark[num_] == 0)
        {
            // ROS_INFO("[%d]:stay_mark has been set to 1", num_);
            stay_pt[num_].linear.x = current_velocity_[num_].twist.linear.x;
            stay_pt[num_].linear.y = current_velocity_[num_].twist.linear.y;
            stay_pt[num_].linear.z = 0;
            stay_mark[num_] = 1;
        }
        control_pub_.publish(stay_pt[num_]);

        // ROS_INFO("[%d]:Current Target Pos: ([%f],[%f])", num_, current_pose_[num_].pose.position.x, current_pose_[num_].pose.position.y);
        // ROS_INFO("[%d]:Current Target Pos: ([%f],[%f])", num_, stay_pt[num_].position.x, stay_pt[num_].position.y);
    }

    // 停止
    void stop()
    {
        static geometry_msgs::Twist stop_pt;
        stop_pt.linear.x = 0;
        stop_pt.linear.y = 0;
        stop_pt.linear.z = 0;
        control_pub_.publish(stop_pt);
    }

    void step(int sigma, double t)
    {
        // static double last_t;
        double ux = 0, uy = 0;
        h_t h_current;
        Eigen::VectorXd avoid_feedback_value = avoid_feedback(current_pose_, obs_pos, num_);
        Eigen::VectorXd diff_feedback = (avoid_feedback_value - feedback_temp.col(num_-1)) / 0.01;
        feedback_temp.col(num_-1) = avoid_feedback_value;

        // 编队向量 0~2为跟随者无人机 3为无人车 hpx hvx hax hpy hvy hay  
        h_current = get_funh(num_, t);

        // 加入防碰撞反馈
        h_current.px += avoid_feedback_value(0);
        h_current.py += avoid_feedback_value(1);
        h_current.vx += diff_feedback(0);
        h_current.vy += diff_feedback(1); 

        // 编队补偿 
        // Eigen::MatrixXd tau = (Eigen::MatrixXd(2,1) << h_current.ax - alpha_p_uav * h_current.px - alpha_v_uav * h_current.vx, 
        //                                                h_current.ay - alpha_p_uav * h_current.py - alpha_v_uav * h_current.vy).finished(); 

        Eigen::MatrixXd tau = (Eigen::MatrixXd(2,1) << h_current.vx, h_current.vy).finished();

        // 状态量
        Eigen::MatrixXd state = (Eigen::MatrixXd(2,1) << 
                current_pose_[num_].pose.position.x, current_pose_[num_].pose.position.y).finished();
        // 拓展编队向量
        Eigen::MatrixXd phi= (Eigen::MatrixXd(2,1) << 
                h_current.px, h_current.py).finished();
        // 输入量
        Eigen::MatrixXd input = K1_ugv * (state - phi) + K2_ugv * obrv_esti.col(num_-1) + tau;
        
        // Ax 除了控制输入外的部分
        ux += input(0,0);
        uy += input(1,0);

        // 观测器
        double error_vx = 0;
        double error_vy = 0;
        // 跟随者之间
        for(int i = 0; i < N_drone+N_UGV-1; i++)
        {
            if(Wmat[sigma](num_, i+1))
            {
                error_vx += obrv_esti(0, num_-1) - obrv_esti(0, i);   // PX
                error_vy += obrv_esti(2, num_-1) - obrv_esti(2, i);   // PY
            }
        }
        // 跟随者与领导者
        if(Wmat[sigma](num_, 0))
        {
            error_vx += obrv_esti(0, num_-1) - current_pose_[0].pose.position.x;
            error_vy += obrv_esti(2, num_-1) - current_pose_[0].pose.position.y;
        }

        Eigen::MatrixXd error = (Eigen::MatrixXd(2,1) << error_vx, error_vy).finished();
        Eigen::MatrixXd diff_v = S*obrv_esti.col(num_-1) - Ko*error;

        obrv_esti.col(num_-1) += diff_v * 0.01; // 积分

        static geometry_msgs::Twist step_pt;
        step_pt.linear.x = ux;
        step_pt.linear.y = uy;
        step_pt.linear.z = 0;
        control_pub_.publish(step_pt);
    }
};

class Obstacler  {
    private:
        // 订阅话题和发布话题
        ros::Subscriber obs_pose_sub;

    public:
        std::string name_;
        // ROS节点句柄
        ros::NodeHandle nh_;
        geometry_msgs::PoseArray current_pose; // geometry_msgs/PoseArray

        Obstacler(const std::string& name) : name_(name) {
            // 初始化ROS订阅者和发布者
            obs_pose_sub = nh_.subscribe<geometry_msgs::PoseArray>("/obstacles", 1000, &Obstacler::obs_poseCallback, this);
        }

        ~Obstacler(void){}

        // 回调函数 ############################################################################
    // 障碍物位置数据回调函数
    void obs_poseCallback(const geometry_msgs::PoseArray::ConstPtr& msg) {
        current_pose = *msg;
        calculate_obs();
    }

    void calculate_obs(void){
        for(int i = 0; i < N_obs; i++) 
        {
            obs_pos(0, i) = current_pose.poses[i].position.x;
            obs_pos(1, i) = current_pose.poses[i].position.y;
        }
    }
};  

// 指令回调函数 ######################################################################
int state = 0;
void commandCallback(const std_msgs::Int32::ConstPtr& msg) {
    std_msgs::Int32 current_command; 
    // 更新无人机状态
    current_command = *msg;
    if (current_command.data == 1)
    {
        ROS_INFO("Current state: Takeoff");
        state = 1;
    }
    else if (current_command.data == 9)
    {
        ROS_INFO("Current state: Landing");
        state = 9;
    }
    else if (current_command.data == 2)
    {
        ROS_INFO("Current state: Running Algorithm");
        state = 2;
    }
    else if (current_command.data == 3)
    {
        ROS_INFO("Current state: Stop Moving");
        state = 3;
    }
}

int main(int argc, char** argv) 
{
    init_topo();

    // 初始化ROS节点
    ros::init(argc, argv, "algorithm");
    // 创建ROS节点句柄
    ros::NodeHandle nh;
    ros::Rate rate(10);
    ros::Rate algorithm_rate(100);
    ros::Subscriber command_sub;    // 控制指令订阅

    command_sub = nh.subscribe<std_msgs::Int32>("/command", 1000, &commandCallback);

    int algorithm_startmark = 0;    // 算法开始的标志，用于计时
    int sigma = 0;  

    // 创建无人机-无人车对象
    Leader  UAV0("1");
    Follower_UAV UAV1("2");
    Follower_UAV UAV2("3");
    Follower_UAV UAV3("4");
    Follower_UGV UGV1("1");
    // Follower_UAV UAV4("5");
    Obstacler OBS("100");

    ROS_INFO("Launch successfully");

    // 准备阶段
    for(int i = 10; ros::ok() && i > 0; --i){
        UAV0.stop();
        UAV1.stop();
        UAV2.stop();
        UAV3.stop();
        // UAV4.stop();
        UGV1.stop();
        rate.sleep();
        ros::spinOnce();
    }
    ROS_INFO("Having sent pre-offboard messages successfully");
    ROS_INFO("Default state: Stay");

    // 确保在ROS节点初始化之后调用ros::Time::now()
    ros::Time start_time, current_time, switch_time;
    float exp_time, from_switch_time;
    while(ros::ok()) {
        // ROS_INFO("Current time: %f", exp_time);
        // 均板外
        if (UAV0.checkoffboard() && UAV1.checkoffboard_fol_uav() && UAV2.checkoffboard_fol_uav() && UAV3.checkoffboard_fol_uav())
        {   
            // 指令顺序应该为1-2-3-9
            // 保持，初始状态为静止
            if (state == 0)
            {
                UAV0.stay();
                UAV1.stay();
                UAV2.stay();
                UAV3.stay();
                // UAV4.stay();
                UGV1.stay();
            }
            // 起飞
            if (state == 1)
            {
                UAV0.takeoff();
                UAV1.takeoff();
                UAV2.takeoff();
                UAV3.takeoff();
                // UAV4.takeoff();
            }
            else if (state == 2)
            {
                // 开始运行算法
                if (algorithm_startmark == 0)
                {
                    switch_time = ros::Time::now(); // 切换时刻
                    start_time = ros::Time::now();  // 实验开始的时刻
                    algorithm_startmark = 1;
                    ROS_INFO("Algorithm Start Running");
                    ROS_INFO("Control Rate has been set to 100Hz");
                }
                exp_time = ros::Time::now().toSec() - start_time.toSec();           // 实验时间
                from_switch_time = ros::Time::now().toSec() - switch_time.toSec();  // 距离上次切换后经过的时间
                // 若达到了切换间隔时间
                if (from_switch_time >= switch_interval * weight_switch[sigma])
                {
                    sigma++;
                    sigma %= n_sigma;      // 随机切换拓扑
                    switch_time = ros::Time::now();     // 更新切换时刻
                }
                ROS_INFO("Running for: %fs, sigma now: %d", exp_time, sigma);       // 当前拓扑图索引
                // 规划部分
                UAV0.step();
                UAV1.step(sigma, exp_time);
                UAV2.step(sigma, exp_time);
                UAV3.step(sigma, exp_time);
                // UAV4.step(sigma, exp_time);
                UGV1.step(sigma, exp_time);
                algorithm_rate.sleep();
            }
            // 停止环绕
            else if (state == 3)
            {
                UAV0.stop();
                UAV1.stop();
                UAV2.stop();
                UAV3.stop();
                // UAV4.stop();
                UGV1.stop();
                rate.sleep();
            }
            // 着陆
            else if (state == 9)
            {
                UAV0.land();
                UAV1.land();
                UAV2.land();
                UAV3.land();
                // UAV4.land();
                rate.sleep();
            }
        }
        // 没有板外，调整控制模式
        else
        {
            ROS_INFO("Sending pre-offboard messages");
            for (int i = 100; ros::ok() && i > 0; --i)
            {
                UAV0.stop();
                UAV1.stop();
                UAV2.stop();
                UAV3.stop();
                // UAV4.stop();
                UGV1.stop();
                rate.sleep();
                ros::spinOnce();
            }
            ROS_INFO("Trying to turning offboard");
            UAV0.setoffboard();
            UAV1.setoffboard_fol_uav();
            UAV2.setoffboard_fol_uav();
            UAV3.setoffboard_fol_uav();
            // UAV4.setoffboard_fol_uav();
            ROS_INFO("Current state: Stay");
            state = 0;
            rate.sleep();
        }
        ros::spinOnce();
    }
    return 0;
}